Advanced Lane Finding

The Project Goals and Objectives

  • Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.
  • Apply a distortion correction to raw images.
  • Use color transforms, gradients, etc., to create a thresholded binary image.
  • Apply a perspective transform to rectify binary image ("birds-eye view").
  • Detect lane pixels and fit to find the lane boundary.
  • Determine the curvature of the lane and vehicle position with respect to center.
  • Warp the detected lane boundaries back onto the original image.
  • Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.
In [5]:
import cv2
import glob
import numpy as np
import matplotlib.pyplot as plt

from moviepy.editor import VideoFileClip
from IPython.display import HTML
%matplotlib inline

1.Camera Calibration

In [6]:
# Make a list of calibration images and images path
doub_images = list(map(lambda imgName: (imgName, cv2.cvtColor(cv2.imread(imgName), cv2.COLOR_BGR2RGB)), 
                                        glob.glob('./camera_cal/*.jpg')))

fig, axes = plt.subplots(4, 5, figsize=(25, 12))  # 4 * 5 image architecture

# show 20 images and path name
for index, ax in zip(range(4*5), axes.flat):
    if index < len(doub_images):
        imgPath, image = doub_images[index]
        ax.set_title(imgPath)
        ax.imshow(image)

Build the show image function:

In [7]:
def showimg(img1, img2, name_img1, name_img2, x, y, img3=[], lbl3=[], cmap=None, n = 2):    
    f, (ax1, ax2) = plt.subplots(1, n, figsize=(x,y))
    ax1.imshow(img1, cmap=cmap)
    ax1.set_title(name_img1, fontsize=30)
    ax2.imshow(img2, cmap=cmap)
    ax2.set_title(name_img2, fontsize=30)
    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

We have 20 images of a chessboard, OpenCV functions provides cv2. FindChessboardCorners () and cv2. CalibrateCamera calibration () for image calibration.

cv2.findChessboardCorners(): The function attempts to determine whether the input image is a view of the chessboard pattern and locate the internal chessboard corners.

cv2.calibrateCamera(): The function estimates the intrinsic camera parameters and extrinsic parameters for each of the views.

In [8]:
def calibrate_camera():
    objpoints = []  # 3d points in real world space
    imgpoints = []  # 2d points in image plane.

    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    objp = np.zeros((9*6, 3), np.float32)
    objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1, 2)
    
    # Make a list of calibration images
    cal_images = glob.glob('camera_cal/calibration*.jpg')
    
    # Step through the list and search for chessboard corners
    for fname in cal_images:
        img = cv2.imread(fname)
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # Find the chessboard corners
        ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
        
        # If found, add object points, image points
        if ret == True:
            objpoints.append(objp)
            imgpoints.append(corners)

    # Do camera calibration given object points and image points
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
    
    return mtx, dist
In [9]:
mtx, dist = calibrate_camera()

2.Apply Distortion Correction

cv2.undistort:The function transforms an image to compensate radial and tangential lens distortion.

In [10]:
test1 = cv2.imread('camera_cal/calibration1.jpg')

undist = cv2.undistort(test1, mtx, dist, None, mtx)
showimg(test1, undist, 'Original image', 'Distortion corrected image', 15, 4)
In [11]:
test5 = cv2.cvtColor(cv2.imread('test_images/test5.jpg'), cv2.COLOR_BGR2RGB)

undist_test5 = cv2.undistort(test5, mtx, dist, None, mtx)
showimg(test5, undist_test5, 'Original image', 'Distortion corrected image', 15, 4)

3: Apply a Perspective Transform

In [12]:
# %matplotlib qt

# source image points
plt.imshow(undist_test5)
plt.plot(753, 478, '.')  # right_top
plt.plot(1075, 670, '.') # right_bottom
plt.plot(216, 688, '.')  # left_bottom
plt.plot(549, 483, '.')  # left_top
Out[12]:
[<matplotlib.lines.Line2D at 0xe1d32b0>]
In [13]:
def warp(img):
    
    # Define calibration box in source (original) and destination (desired or warped) coordinates
    img_size = (img.shape[1], img.shape[0])
    
#     # Another ways.
#     src = np.float32([[(img_size[0] / 2) - 55, img_size[1] / 2 + 100],
#                       [((img_size[0] / 6) - 10), img_size[1]],
#                       [(img_size[0] * 5 / 6) + 60, img_size[1]],
#                       [(img_size[0] / 2 + 55), img_size[1] / 2 + 100]])
    
#     dst = np.float32([[(img_size[0] / 4), 0],
#                       [(img_size[0] / 4), img_size[1]],
#                       [(img_size[0] * 3 / 4), img_size[1]],
#                       [(img_size[0] * 3 / 4), 0]])
    
    # Four source coordinates
    src = np.float32([[753, 478],
                     [1092, 670],
                     [216, 688],
                     [549, 483]])
    
    # Four desired coordinates
    dst = np.float32([[997, 1],
                     [982, 712],
                     [316, 712],
                     [366, 1]])
    
    # Compute the perspective transform , M 
    M = cv2.getPerspectiveTransform(src, dst)
    
    # Could compute the inverse also by swapping the input parameters
    Minv = cv2.getPerspectiveTransform(dst, src)
    
    # Create warped image - uses linear interpolation
    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)
    
    return warped, M, Minv
Display effect:
In [14]:
%matplotlib inline

# Used perspective transform all test_images
test_images = glob.glob('test_images/*.jpg')

for i in range(len(test_images)):
    test_image = cv2.cvtColor(cv2.imread(test_images[i]), cv2.COLOR_RGB2BGR)
    undist_test = cv2.undistort(test_image, mtx, dist, None, mtx)
    warped_undist_test, M, Minv = warp(undist_test)
    showimg(undist_test, warped_undist_test, 'Original image', 'Warped image', 15, 4)
Decide to use the lane line:
In [15]:
test4 = cv2.cvtColor(cv2.imread(test_images[6]), cv2.COLOR_RGB2BGR)
undist_test4 = cv2.undistort(test4, mtx, dist, None, mtx)
warped_undist_test4 = warp(undist_test4)[0]
plt.imshow(warped_undist_test4)
Out[15]:
<matplotlib.image.AxesImage at 0xe144898>

4: Create a Thresholded Binary Image

Now let's explore four color Spaces: RGB, HSV, HLS and LAB to detect the lane lines in the image.

4.1 RGB color space:

RGB is red-green-blue color space. You can think of this as a 3D space, in this case a cube, where any color can be represented by a 3D coordinate of R, G, and B values. For example, white has the coordinate (255, 255, 255), which has the maximum value for red, green, and blue.

In [16]:
warped_undist_test4_RGB = warped_undist_test4
warped_undist_test4_RGB_R = warped_undist_test4_RGB[:,:,0]
warped_undist_test4_RGB_G = warped_undist_test4_RGB[:,:,1]
warped_undist_test4_RGB_B = warped_undist_test4_RGB[:,:,2]
Display effect:
In [17]:
showimg(warped_undist_test4_RGB, warped_undist_test4_RGB_R, 'Original RGB image', 'R-Channel', 15, 4)
showimg(warped_undist_test4_RGB_G, warped_undist_test4_RGB_B, 'G-Channel', 'B-Channel', 15, 4)

4.2 HSV color space:

HSV (Hue, Saturation, Value):The HSV representation models the way paints of different colors mix together, with the saturation dimension resembling various shades of brightly colored paint, and the value dimension resembling the mixture of those paints with varying amounts of black or white paint.

In [18]:
warped_undist_test4_HSV = cv2.cvtColor(warped_undist_test4_RGB, cv2.COLOR_RGB2HSV)
warped_undist_test4_HSV_H = warped_undist_test4_HSV[:,:,0]
warped_undist_test4_HSV_S = warped_undist_test4_HSV[:,:,1]
warped_undist_test4_HSV_V = warped_undist_test4_HSV[:,:,2]
Display effect:
In [19]:
showimg(warped_undist_test4_HSV, warped_undist_test4_HSV_H, 'Original HSV image', 'H-Channel', 15, 4)
showimg(warped_undist_test4_HSV_S, warped_undist_test4_HSV_V, 'S-Channel', 'V-Channel', 15, 4)

4.3 HLS color space:

HSL (Hue, Saturation, Lightness):The HSL model attempts to resemble more perceptual color models such as NCS or Munsell, placing fully saturated colors around a circle at a lightness value of 1/2, where a lightness value of 0 or 1 is fully black or white, respectively.

In [20]:
warped_undist_test4_HLS = cv2.cvtColor(warped_undist_test4_RGB, cv2.COLOR_RGB2HLS)
warped_undist_test4_HLS_H = warped_undist_test4_HLS[:,:,0]
warped_undist_test4_HLS_L = warped_undist_test4_HLS[:,:,1]
warped_undist_test4_HLS_S = warped_undist_test4_HLS[:,:,2]
Display effect:
In [21]:
showimg(warped_undist_test4_HLS, warped_undist_test4_HLS_H, 'Original HLS image', 'H-Channel', 15, 4)
showimg(warped_undist_test4_HLS_L, warped_undist_test4_HLS_S, 'L-Channel', 'S-Channel', 15, 4)

4.4 LAB color space:

The Lab color space describes mathematically all perceivable colors in the three dimensions L for lightness and a and b for the color opponents green–red and blue–yellow.

In [22]:
warped_undist_test4_LAB = cv2.cvtColor(warped_undist_test4_RGB, cv2.COLOR_RGB2LAB)
warped_undist_test4_LAB_L = warped_undist_test4_LAB[:,:,0]
warped_undist_test4_LAB_A = warped_undist_test4_LAB[:,:,1]
warped_undist_test4_LAB_B = warped_undist_test4_LAB[:,:,2]
Display effect:
In [23]:
showimg(warped_undist_test4_LAB, warped_undist_test4_LAB_L, 'Original LAB image', 'LAB L-Channel', 15, 4)
showimg(warped_undist_test4_LAB_A, warped_undist_test4_LAB_B, 'LAB A-Channel', 'LAB B-Channel', 15, 4)

4.5 Create Two Color Space Threshold Function

From the above diagram, the three color channels of LAB can be seen that the left lane line of the B channel is clearly indicated, while the lane line on the right side of the L channel in HLS is more obvious.We'll apply LAB B-threshold and HLS L-threshold to the image to highlight the lane lines.

In [37]:
def lab_b_channel(img):
    
    # 1) Apply a threshold to the b channel
    img = img[:, :, 2]
    if np.max(img) > 180:
        img = img*(255/np.max(img))

    b_thresh_min = 190
    b_thresh_max = 255
    
    # 2) Return a binary image of threshold result
    binary_output = np.zeros_like(img)
    binary_output[(img > b_thresh_min) & (img <= b_thresh_max)] = 1
    return binary_output
Display effect:
In [38]:
lab_b_channel_test4 = lab_b_channel(warped_undist_test4_LAB)
showimg(warped_undist_test4_LAB, lab_b_channel_test4, 'LAB image', 'B-thresholded LAB image', 15, 4, cmap = 'gray')
In [39]:
def hls_l_channel(img):
    
    # 1) Apply a threshold to the S channel
    l_img = img[:,:,1]
    l_img = l_img*(255/np.max(l_img))
    
    l_thresh_min = 210
    l_thresh_max = 255
    
    # 2) Return a binary image of threshold result
    binary_output = np.zeros_like(l_img)
    binary_output[(l_img > l_thresh_min) & (l_img <= l_thresh_max)] = 1
    return binary_output
Display effect:
In [40]:
hls_l_channel_test4 = hls_l_channel(warped_undist_test4_HLS)
showimg(warped_undist_test4_HLS, hls_l_channel_test4, 'HLS_L image', 'L-thresholded HLS image', 15, 4, cmap = 'gray')

4.6 Combined Two Color Space Threshold:

In [41]:
def combined_threshold(warped_undist_image):

    # Convert to HVS color space
    img_LAB = cv2.cvtColor(warped_undist_image, cv2.COLOR_RGB2LAB)

    # Convert to HLS color space
    img_HLS = cv2.cvtColor(warped_undist_image, cv2.COLOR_RGB2HLS)
    
    # Apply a threshold to the s channel
    img_b_LAB = lab_b_channel(img_LAB)
    # Apply a threshold to the l channel
    img_l_HLS = hls_l_channel(img_HLS)

    # Threshold x gradient
    combined_img = np.zeros_like(img_l_HLS)
    combined_img[(img_l_HLS == 1)|(img_b_LAB == 1)] = 1
    return combined_img
Display effect:
In [42]:
threshold_color_img = combined_threshold(warped_undist_test4)
showimg(warped_undist_test4, threshold_color_img, 'RGB image', 'Thresholded image', 15, 4, cmap = 'gray')

The effect is very good. In addition to using color channel to determine the threshold, Sobel can also be used, but there will be more noise after Sobel processing, so only color channel is used here.

5: Create the Image Processing Pipeline Function

In [43]:
def pipeline_process(img):
    
    # Undistort
    undistort_img = cv2.undistort(img, mtx, dist, None, mtx)
    
    # Perspective Transform
    warp_undist_img, M, Minv = warp(undistort_img)

    # Create a thresholded binary image
    thresh_img = combined_threshold(warp_undist_img)
    
    return thresh_img, Minv
Display effect:
In [44]:
test_images_i = []
test_images_processed = []
test_images = glob.glob('test_images/*.jpg')

for test_image in test_images:
    or_img = cv2.cvtColor(cv2.imread(test_image), cv2.COLOR_BGR2RGB)
    test_images_i.append(or_img)
    image, Minv = pipeline_process(or_img)
    test_images_processed.append(image)
    showimg(or_img, image, 'Original image', 'pipeline image', 15, 4, cmap='gray')

6: Detect The Lane Lines

I first take a histogram along all the columns in the lower half of the image like this:

In [28]:
histogram = np.sum(threshold_color_img[threshold_color_img.shape[0]//2:,:], axis=0)
plt.plot(histogram)
Out[28]:
[<matplotlib.lines.Line2D at 0x1691a0f0>]

With this histogram I am adding up the pixel values along each column in the image. In my thresholded binary image, pixels are either 0 or 1, so the two most prominent peaks in this histogram will be good indicators of the x-position of the base of the lane lines. I can use that as a starting point for where to search for the lines. From that point, I can use a sliding window, placed around the line centers, to find and follow the lines up to the top of the frame.

In [29]:
def sliding_window(img):
    """
    Fit a polynomial to the input binary image.
    
    Receive: 
        img: Processed images.
    
    Return: 
        left_fit, right_fit : It fits the second order polynomial on the left and the right.
        left_lane_inds, right_lane_inds : location index of the left and right lane lines.
        visualization_data : Save a certain amount of sliding window width and height and
                                Take a histogram of the bottom half of the image As a tuple.
    """

    # Take a histogram of the bottom half of the image
    histogram = np.sum(img[img.shape[0]//2:,:], axis=0)
    # Find the peak of the left and right halves of the histogram
    # These will be the starting point for the left and right lines
    midpoint = np.int(histogram.shape[0]//2)
    quarter_point = np.int(midpoint//2)
    # Previously the left/right base was the max of the left/right half of the histogram
    # this changes it so that only a quarter of the histogram (directly to the left/right) is considered
    leftx_base = np.argmax(histogram[quarter_point:midpoint]) + quarter_point
    rightx_base = np.argmax(histogram[midpoint:(midpoint+quarter_point)]) + midpoint
    
    # Choose the number of sliding windows
    nwindows = 50
    # Set height of windows
    window_height = np.int(img.shape[0]/nwindows)
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    # Current positions to be updated for each window
    leftx_current = leftx_base
    rightx_current = rightx_base
    # Set the width of the windows +/- margin
    margin = 80
    # Set minimum number of pixels found to recenter window
    minpix = 40
    # Create empty lists to receive left and right lane pixel indices
    left_lane_inds = []
    right_lane_inds = []
    # Rectangle data for visualization
    rectangle_data = []

    # Step through the windows one by one
    for window in range(nwindows):
        # Identify window boundaries in x and y (and right and left)
        win_y_low = img.shape[0] - (window+1)*window_height
        win_y_high = img.shape[0] - window*window_height
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin
        rectangle_data.append((win_y_low, win_y_high, win_xleft_low, win_xleft_high, win_xright_low, win_xright_high))
        # Identify the nonzero pixels in x and y within the window
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) &
                          (nonzerox < win_xleft_high)).nonzero()[0]
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) &
                           (nonzerox < win_xright_high)).nonzero()[0]
        # Append these indices to the lists
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)
        # If you found > minpix pixels, recenter next window on their mean position
        if len(good_left_inds) > minpix:
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
        if len(good_right_inds) > minpix:        
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))

    # Concatenate the arrays of indices
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)

    # Extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds] 
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds] 

    left_fit, right_fit = (None, None)
    # Fit a second order polynomial to each
    if len(leftx) != 0:
        left_fit = np.polyfit(lefty, leftx, 2)
    if len(rightx) != 0:
        right_fit = np.polyfit(righty, rightx, 2)
    
    visualization_data = (rectangle_data, histogram)
    
    return left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data
Display effect:
In [30]:
# Visualize the sliding windows over an example test image.
img = test_images_processed[0]
left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data = sliding_window(img)
rectangles = visualization_data[0]
histogram = visualization_data[1]
# Create an output image to draw on and visualize the result
out_img = np.uint8(np.dstack((img, img, img))*255)
# Generate x and y values for plotting
ploty = np.linspace(0, img.shape[0]-1, img.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
for rect in rectangles:
    # Draw the windows on the visualization image
    cv2.rectangle(out_img,(rect[2],rect[0]),(rect[3],rect[1]),(0,255,0), 2) 
    cv2.rectangle(out_img,(rect[4],rect[0]),(rect[5],rect[1]),(0,255,0), 2) 
# Identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [100, 200, 255]
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
plt.subplot(1, 2, 2)
# show the out_img image
plt.imshow(out_img)
plt.xlabel('Sliding window', fontsize=15)
plt.plot(left_fitx, ploty, color='yellow')
plt.plot(right_fitx, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()

6.2 Another way:Skip the sliding windows step once you know where the lines are

once you know where the lines are in one frame of video, you can do a highly targeted search for them in the next frame. This is equivalent to using a customized region of interest for each frame of video, and should help you track the lanes through sharp curves and tricky conditions.

In [31]:
def polyfit_fit(img, left_fit, right_fit):
    """
    Fit a polynomial to the input binary image based upon a previous fit.
    
    Receive Parameters: 
        img: Processed images.
        left_fit, right_fit: It fits the second order polynomial on the left and the right.
        
    Return Parameters:
        left_fit_new, right_fit_new: It new fits the second order polynomial on the left and the right.
        left_lane_inds, right_lane_inds:  location index of the left and right lane lines.
    """

    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    margin = 100
    left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] - margin)) & 
                      (nonzerox < (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + left_fit[2] + margin))) 

    right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] - margin)) & 
                       (nonzerox < (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + right_fit[2] + margin)))  

    # Again, extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds]
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    
    # Fit a second order polynomial to each
    left_fit_new, right_fit_new = (None, None)
    if len(leftx) != 0:
        left_fit_new = np.polyfit(lefty, leftx, 2)
    if len(rightx) != 0:
        right_fit_new = np.polyfit(righty, rightx, 2)
    return left_fit_new, right_fit_new, left_lane_inds, right_lane_inds
Display effect:
In [32]:
# Visualize the polyfit_prev_fit over the an example image.
margin = 50
img = test_images_processed[0]
left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data = sliding_window(img)
left_fit2, right_fit2, left_lane_inds2, right_lane_inds2 = polyfit_fit(img, left_fit, right_fit)
ploty = np.linspace(0, img.shape[0]-1, img.shape[0] )
left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
left_fitx2 = left_fit2[0]*ploty**2 + left_fit2[1]*ploty + left_fit2[2]
right_fitx2 = right_fit2[0]*ploty**2 + right_fit2[1]*ploty + right_fit2[2]
# Create an image to draw on and an image to show the selection window
out_img = np.uint8(np.dstack((img, img, img))*255)
window_img = np.zeros_like(out_img)
# Identify the x and y positions of all nonzero pixels in the image
nonzero = img.nonzero()
nonzeroy = np.array(nonzero[0])
nonzerox = np.array(nonzero[1])
# Color in left and right line pixels
out_img[nonzeroy[left_lane_inds2], nonzerox[left_lane_inds2]] = [255, 0, 0]
out_img[nonzeroy[right_lane_inds2], nonzerox[right_lane_inds2]] = [0, 0, 255]
# Generate a polygon to illustrate the search window area
# And recast the x and y points into usable format for cv2.fillPoly()
left_line_window1 = np.array([np.transpose(np.vstack([left_fitx-margin, ploty]))])
left_line_window2 = np.array([np.flipud(np.transpose(np.vstack([left_fitx+margin, ploty])))])
left_line_pts = np.hstack((left_line_window1, left_line_window2))
right_line_window1 = np.array([np.transpose(np.vstack([right_fitx-margin, ploty]))])
right_line_window2 = np.array([np.flipud(np.transpose(np.vstack([right_fitx+margin, ploty])))])
right_line_pts = np.hstack((right_line_window1, right_line_window2))
# Draw the lane onto the warped blank image
cv2.fillPoly(window_img, np.int_([left_line_pts]), (0,255, 0))
cv2.fillPoly(window_img, np.int_([right_line_pts]), (0,255, 0))
result = cv2.addWeighted(out_img, 1, window_img, 0.3, 0)
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
# show the result image
plt.subplot(1, 2, 2)
plt.imshow(result)
plt.xlabel('Polyfit using previous fit', fontsize=15)
plt.plot(left_fitx2, ploty, color='yellow')
plt.plot(right_fitx2, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()

7: Measuring Curvature

step 1.The radius of curvature at any point x of the function x=f(y) is given as follows:

$$ R_{curve}= \frac{[1+(\frac{{\rm d}x}{{\rm d}y})^2]^{3/2}}{\left| \frac{{\rm d}^2x}{{\rm d}y^2} \right|} $$

In the case of the second order polynomial above, the first and second derivatives are:

$ f'(y)=\frac{{\rm d}x}{{\rm d}y}=2Ay+B $

$ f''(y)=\frac{{\rm d}^2x}{{\rm d}y^2}=2A $

So, our equation for radius of curvature becomes:

$$ R_{curve}= \frac{(1+(2Ay+B)^2)^{3/2}}{\left| {2A} \right|} $$

In the code, the specific equation is as follows:

((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])

left_fit_cr[0]: on behalf of A. is the first coefficient (the y-squared coefficient) of the second order polynomial fit.

left_fit_cr[1]: on behalf of B. is the second coefficient.

y_eval*ym_per_pix: on behalf of y. y_eval is the y position within the image upon which the curvature calculation is based. y_meters_per_pixel is the factor used for converting from pixels to meters.

step 2.Then calculate the code of the vehicle in the center of the lane:

l_fit_x_int = l_fit[0]*h**2 + l_fit[1]*h + l_fit[2]

r_fit_x_int = r_fit[0]*h**2 + r_fit[1]*h + r_fit[2]

lane_center_position = (r_fit_x_int + l_fit_x_int) /2

center_dist = (car_position - lane_center_position) * xm_per_pix

l_fit_x_int and r_fit_x_int are the x-intercepts of the left and right fits.The car position is the difference between these intercept points and the image midpoint (assuming that the camera is mounted at the center of the vehicle).

In [33]:
def curv_pos(img, l_fit, r_fit, l_lane_inds, r_lane_inds):
    """
    Calculating the lane curvature and the vehicle position on the lane.
    
    Receive Parameters: 
        img: Processed images.
        l_fit, r_fit: It fits the second order polynomial on the left and the right.
        l_lane_inds, r_lane_inds: location index of the left and right lane lines.
        
    Return Parameters:
        left_curverad :  left lane curvature.
        right_curverad : right lane curvature.
        center_dist : The approximate location of the camera.
    """

    # Define conversions in x and y from pixels space to meters
    ym_per_pix = 3.048/100 # meters per pixel in y dimension
    xm_per_pix = 3.7/378 # meters per pixel in x dimension
    left_curverad, right_curverad, center_dist = (0, 0, 0)
    # Define y-value where we want radius of curvature
    # I'll choose the maximum y-value, corresponding to the bottom of the image
    h = img.shape[0]
    ploty = np.linspace(0, h-1, h)
    y_eval = np.max(ploty)
  
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = img.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    # Again, extract left and right line pixel positions
    leftx = nonzerox[l_lane_inds]
    lefty = nonzeroy[l_lane_inds] 
    rightx = nonzerox[r_lane_inds]
    righty = nonzeroy[r_lane_inds]
    
    if len(leftx) != 0 and len(rightx) != 0:
        # Fit new polynomials to x,y in world space
        left_fit_cr = np.polyfit(lefty*ym_per_pix, leftx*xm_per_pix, 2)
        right_fit_cr = np.polyfit(righty*ym_per_pix, rightx*xm_per_pix, 2)
        # Calculate the new radii of curvature
        # Now our radius of curvature is in meters
        left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
        right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
        
    # Distance from center is image x midpoint - mean of l_fit and r_fit intercepts 
    if r_fit is not None and l_fit is not None:
        car_position = img.shape[1]/2
        l_fit_x_int = l_fit[0]*h**2 + l_fit[1]*h + l_fit[2]
        r_fit_x_int = r_fit[0]*h**2 + r_fit[1]*h + r_fit[2]
        lane_center_position = (r_fit_x_int + l_fit_x_int) /2
        center_dist = (car_position - lane_center_position) * xm_per_pix
    return left_curverad, right_curverad, center_dist
Display effect:
In [34]:
# Visualize the curv_pos over the an example image.
rad_l, rad_r, d_center = curv_pos(test_images_processed[0], left_fit, right_fit, left_lane_inds2, right_lane_inds2)
print('Radius of curvature for example:', rad_l, 'm,', rad_r, 'm')
print('Distance from lane center for example:', d_center, 'm')
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
plt.subplot(1, 2, 2)
# show the result image
plt.imshow(result)
plt.xlabel('Polyfit using previous fit', fontsize=15)
plt.plot(left_fitx2, ploty, color='yellow')
plt.plot(right_fitx2, ploty, color='yellow')
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()

Radius of curvature for example: 2719.57596707 m, 3226.04186033 m
Distance from lane center for example: 0.00313699671096 m

8: Drawing the Lane Boundaries

In [40]:
def draw_lane(or_img, binary_img, l_fit, r_fit, Minv):
    """
    Draw the detected lane over the input image.
    
        Receive Parameters: 
            or_img: Original input image.
            binary_img: Preprocessed image.
            l_fit, r_fit: Detected lanes.
            Minv: Calibration matrix.
        
        Return Parameters:
            result : A picture with a Lane Boundaries.
    """
    new_img = np.copy(or_img)
    if l_fit is None or r_fit is None:
        return or_img
    
    # Create an image to draw the lines on
    warp_zero = np.zeros_like(binary_img).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))
    h,w = binary_img.shape
    ploty = np.linspace(0, h-1, num=h)
    left_fitx = l_fit[0]*ploty**2 + l_fit[1]*ploty + l_fit[2]
    right_fitx = r_fit[0]*ploty**2 + r_fit[1]*ploty + r_fit[2]
    
    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
    pts = np.hstack((pts_left, pts_right))
    
    # Draw the lane onto the warped blank image
    cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
    cv2.polylines(color_warp, np.int32([pts_left]), isClosed=False, color=(255,0,255), thickness=15)
    cv2.polylines(color_warp, np.int32([pts_right]), isClosed=False, color=(0,255,255), thickness=15)
    newwarp = cv2.warpPerspective(color_warp, Minv, (w, h)) 
    result = cv2.addWeighted(new_img, 1, newwarp, 0.5, 0)
    return result

8.1 Write Lane Curvature to image:

In [38]:
def write_data(or_img, curv_rad, center_dist):
    """
    Write the lane curvature and vehicle position over the input image.
    
        Receive Parameters:
            or_img: Original input image.
            curv_rad: Lane curvature.
            center_dist: Vehicle position.
            
        Return new_img:
            new_img: Show pictures of related parameters.
    
    """

    new_img = np.copy(or_img)
    h = new_img.shape[0]
    font = cv2.FONT_HERSHEY_DUPLEX
    text = 'Curve radius: ' + '{:04.2f}'.format(curv_rad) + 'm'
    cv2.putText(new_img, text, (40,70), font, 1.5, (255,255,255), 2, cv2.LINE_AA)
    direction = ''
    
    if center_dist > 0:
        direction = 'right'
    elif center_dist < 0:
        direction = 'left'
    abs_center_dist = abs(center_dist)
    text = '{:04.3f}'.format(abs_center_dist) + 'm ' + direction + ' of center'
    cv2.putText(new_img, text, (40,120), font, 1.5, (255,255,255), 2, cv2.LINE_AA)
    return new_img
Display effect:
In [39]:
# Use the first photo of the test set.
or_img_i = test_images_i[0]
pro_img = test_images_processed[0]
# Get two lane lines related data.
left_fit, right_fit, left_lane_inds, right_lane_inds, visualization_data = sliding_window(pro_img)
left_fit2, right_fit2, left_lane_inds2, right_lane_inds2 = polyfit_fit(pro_img, left_fit, right_fit)
rad_l, rad_r, d_center = curv_pos(pro_img, left_fit, right_fit, left_lane_inds2, right_lane_inds2)
# draw the lane area
result = draw_lane(or_img_i, pro_img, left_fit, right_fit, Minv)
result = write_data(result, (rad_l+rad_r)/2, d_center)
# show the original image
plt.figure(figsize=(14, 7))
plt.subplot(1, 2, 1)
plt.imshow(test_images_i[0])
plt.xlabel('Original image', fontsize=15)
plt.xticks([])
plt.yticks([])
# show the Draw Lane
plt.subplot(1, 2, 2)
plt.imshow(result)
plt.xlabel('Drawing Lane and parameters', fontsize=15)
plt.xlim(0, 1280)
plt.ylim(720, 0)
plt.xticks([])
plt.yticks([])
plt.show()

9: Process Project Videos

In [41]:
# Define a class to receive the characteristics of each line detection
class Line():
    def __init__(self):
        # was the line detected in the last iteration?
        self.detected = False  
        # x values of the last n fits of the line
        self.recent_xfitted = [] 
        #average x values of the fitted line over the last n iterations
        self.bestx = None     
        #polynomial coefficients averaged over the last n iterations
        self.best_fit = None  
        #polynomial coefficients for the most recent fit
        self.current_fit = []  
        #radius of curvature of the line in some units
        self.radius_of_curvature = None 
        #distance in meters of vehicle center from the line
        self.line_base_pos = None 
        #difference in fit coefficients between last and new fits
        self.diffs = np.array([0,0,0], dtype='float') 
        #number of detected pixels
        self.px_count = None
        
    def add_fit(self, fit, inds):
        if fit is not None:
            self.detected = True
            self.px_count = np.count_nonzero(inds)
            self.current_fit.append(fit)
            if len(self.current_fit) > 5:
                self.current_fit = self.current_fit[len(self.current_fit)-5:]
            self.best_fit = np.average(self.current_fit, axis=0)
        else:
            self.detected = False
            if len(self.current_fit) > 0:
                self.current_fit = self.current_fit[:len(self.current_fit)-1]
            if len(self.current_fit) > 0:
                self.best_fit = np.average(self.current_fit, axis=0)

9.1 Create Video Processor Function

In [42]:
def video_processor(img):
    """
    From the input to the picture direct output has processed the completed lane, curvature, vehicle position picture.
    
    Receive Parameters:
        img: Input frame.
    
    Return Parameters:
        image_output: The output is processed through a series of images.
    """
    undist_img = cv2.undistort(img, mtx, dist, None, mtx)
    bin2_img, Minv = pipeline_process(img)
    if not l_line.detected or not r_line.detected:
        l_fit, r_fit, l_lane_inds, r_lane_inds, _ = sliding_window(bin2_img)
    else:
        l_fit, r_fit, l_lane_inds, r_lane_inds = polyfit_fit(bin2_img, l_line.best_fit, r_line.best_fit)
        
    l_line.add_fit(l_fit, l_lane_inds)
    r_line.add_fit(r_fit, r_lane_inds)
    img_out = draw_lane(undist_img, bin2_img, l_fit, r_fit, Minv)
    rad_l, rad_r, d_center = curv_pos(bin2_img, l_fit, r_fit, l_lane_inds, r_lane_inds)
    image_output = write_data(img_out, (rad_l+rad_r)/2, d_center)
    return image_output

Processing project_video.mp4

Display effect:
In [46]:
l_line = Line()
r_line = Line()
project_video_output = 'project_video_output.mp4'
project_video_input = VideoFileClip('project_video.mp4')
processed_video = project_video_input.fl_image(video_processor)
%time processed_video.write_videofile(project_video_output, audio=False)

HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(project_video_output))

[MoviePy] >>>> Building video project_video_output.mp4
[MoviePy] Writing video project_video_output.mp4

100%|█████████████████████████████████████▉| 1260/1261 [04:12<00:00,  5.09it/s]

[MoviePy] Done.
[MoviePy] >>>> Video ready: project_video_output.mp4 

Wall time: 4min 14s
Out[46]: